#include "config.h"
#include "FreeRTOS.h"
#include "sensors.h"
#include "state.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "qMath.h"
#include "sensors_MPU6050.h"
#include "sensors_HMC5883.h"
#include "quadStability_TASK.h"
#include "remote_TASK.h"
#include "commands.h"

struct Config_t _config;

const q_t debugZeroVal = 0;
const q_t debugOneVal = Q_ONE;

const struct pidValueReference_t pidValueReferences[] = {
    "Zero", 255, &debugZeroVal,
    "One", 255, &debugOneVal,
    "pid0", 0, &STATE.pid[0].values.output,
    "pid1", 1, &STATE.pid[1].values.output,
    "pid2", 2, &STATE.pid[2].values.output,
    "pid3", 3, &STATE.pid[3].values.output,
    "pid4", 4, &STATE.pid[4].values.output,
    "pid5", 5, &STATE.pid[5].values.output,
    "pid6", 6, &STATE.pid[6].values.output,
    "pid7", 7, &STATE.pid[7].values.output,
    "pid8", 8, &STATE.pid[8].values.output,
    "pid9", 9, &STATE.pid[9].values.output,
    "pid10", 10, &STATE.pid[10].values.output,
    "pid11", 11, &STATE.pid[11].values.output,
    "pid12", 12, &STATE.pid[12].values.output,
    "pid13", 13, &STATE.pid[13].values.output,
    "pid14", 14, &STATE.pid[14].values.output,
    "pid15", 15, &STATE.pid[15].values.output,
    "IMU.roll", 255, &STATE.imu.output.roll,
    "IMU.pitch", 255, &STATE.imu.output.pitch,
    "IMU.yaw", 255, &STATE.imu.output.yaw,
    "IMU.rollD", 255, &STATE.imu.output.rollD,
    "IMU.pitchD", 255, &STATE.imu.output.pitchD,
    "IMU.yawD", 255, &STATE.imu.output.yawD,
    "RC.0",255,&STATE.rc.axisQ[0],
    "RC.1",255,&STATE.rc.axisQ[1],
    "RC.2",255,&STATE.rc.axisQ[2],
    "RC.3",255,&STATE.rc.axisQ[3],
    "RC.4",255,&STATE.rc.axisQ[4],
    "RC.5",255,&STATE.rc.axisQ[5],
    "RC.6",255,&STATE.rc.axisQ[6],
    "RC.7",255,&STATE.rc.axisQ[7],
};

#define pidValueReferencesNames "Zero|One|pid0|pid1|pid2|pid3|pid4|pid5|pid6|pid7|pid8|pid9|pid10|pid11|pid12|pid13|pid14|pid15|IMU.roll|IMU.pitch|IMU.yaw|IMU.rollD|IMU.pitchD|IMU.yawD|RC.0|RC.1|RC.2|RC.3|RC.4|RC.5|RC.6|RC.7"

enum CONFIG_VALUE_TYPE {
    CONFIGTYPE_UNSIGNED = 0,
    CONFIGTYPE_SIGNED = 1,
    CONFIGTYPE_BITMASK = 2
};

struct configType_t
{
    BYTE totalBytes;
    BYTE numValues;
    BYTE intBits;
    BYTE floatBits;
    enum CONFIG_VALUE_TYPE valueType;
};

struct configValues_t
{
    const char * name;
    const char * shortName;
    const struct configType_t * type;
    void * target;
    void (* onChange)(const void * const, const void * const);
    const char * enumValues;
};

struct configTab_t
{
    const char * name;
    const char * shortName;
    struct configValues_t * values;
	int valueLength;
    int offset;
};



                                     // bytes, num values, int bits, float bits, type
const struct configType_t intType =       {4  ,1  ,32 ,0  ,CONFIGTYPE_SIGNED};
const struct configType_t ushortType =    {2  ,1  ,16 ,0  ,CONFIGTYPE_UNSIGNED};
const struct configType_t byteType =      {1  ,1  ,8  ,0  ,CONFIGTYPE_UNSIGNED};
const struct configType_t sbyteType =     {1  ,1  ,8  ,0  ,CONFIGTYPE_SIGNED};
const struct configType_t doubleByteType ={2  ,2  ,8  ,0  ,CONFIGTYPE_UNSIGNED};
const struct configType_t q32Type =       {4  ,1  ,16 ,16 ,CONFIGTYPE_SIGNED};
const struct configType_t q32HexType =       {4*8  ,8  ,16 ,16 ,CONFIGTYPE_SIGNED};
const struct configType_t q32DoubleType = {8  ,2  ,16 ,16 ,CONFIGTYPE_SIGNED};
const struct configType_t q32VectorType = {12 ,3  ,16 ,16 ,CONFIGTYPE_SIGNED};
const struct configType_t trippleSbyteType =     {3  ,3  ,8  ,0  ,CONFIGTYPE_SIGNED};
const struct configType_t doubleShortType={4,2,16,0,CONFIGTYPE_SIGNED};
const struct configType_t byteBitMaskType   ={1,1,8,0,CONFIGTYPE_BITMASK};

void quatAccChanged(const void * ref, const void * value)
{
    _config.imu.quatAccSetup[0] = ((char*)value)[0];
    _config.imu.quatAccSetup[1] = ((char*)value)[1];
    _config.imu.quatAccSetup[2] = ((char*)value)[2];
    quatReset();
}
void quatGyroChanged(const void * ref, const void * value)
{
    _config.imu.quatGyroSetup[0] = ((char*)value)[0];
    _config.imu.quatGyroSetup[1] = ((char*)value)[1];
    _config.imu.quatGyroSetup[2] = ((char*)value)[2];
    quatReset();
}
void quatMagnChanged(const void * ref, const void * value)
{
    _config.imu.quatMagnSetup[0] = ((char*)value)[0];
    _config.imu.quatMagnSetup[1] = ((char*)value)[1];
    _config.imu.quatMagnSetup[2] = ((char*)value)[2];
    quatReset();
}

void pidChanged(const void * ref, const void * value)
{
    if (ref != NULL)
        *((BYTE*)ref) = *((BYTE*)value);

    int i;
    for (i=0;i<16;i++)
    {
        CONFIG.pid[i].feedback = &pidValueReferences[CONFIG.pid[i].feedbackRef];
        CONFIG.pid[i].setpoint = &pidValueReferences[CONFIG.pid[i].setpointRef];
    }
}

const struct configValues_t systemValues[] = {
    "IMU Freq mode", "im"           ,&byteType, &_config.imu.useSemaphore, NULL, "Fixed|Sync with MPU6050",
    "IMU Freqency (hz)", "if"       ,&ushortType, &_config.imu.freq, NULL, NULL,
    "IMU Algorithm", "ia"           ,&byteType       ,&_config.imu.filter                    ,NULL, "Complementary|Quaternion",
	"IMU Quaternion - Beta", "qb"   ,&q32Type        ,&_config.imu.quaternionBeta			,NULL,NULL,
    "IMU Quat Acc axis", "qa"       ,&trippleSbyteType,&_config.imu.quatAccSetup             ,&quatAccChanged,NULL,
    "IMU Quat Gyro axis", "qg"      ,&trippleSbyteType,&_config.imu.quatGyroSetup             ,&quatGyroChanged,NULL,
    "IMU Quat Magn axis", "qm"      ,&trippleSbyteType,&_config.imu.quatMagnSetup             ,&quatMagnChanged,NULL,
	"IMU Comp. acc weight", "cw"    ,&q32Type        ,&_config.imu.complementaryAccWeight	,NULL,NULL,
    "Motorcount", "mm"              ,&byteType       ,&_config.base.motors                   ,NULL,NULL,
    "VLipo scale", "vl"             ,&q32Type       ,&_config.base.vLipoScale                   ,NULL,NULL,
    "VLipo warning", "vw"           ,&q32Type       ,&_config.base.vLipoWarning                 ,NULL,NULL,
    "VLipo critical", "vc"           ,&q32Type       ,&_config.base.vLipoCritical                 ,NULL,NULL,
    "Current scale", "cu"           ,&q32Type       ,&_config.base.currentScale                   ,NULL,NULL,
    "Enable motors", "me"           ,&byteType          ,&_config.base.enableMotorOutput            ,NULL,"No|Yes"
};

const struct configValues_t motorMappingValues[] = {
    "Motor 0 =", "m0", &byteType, &_config.base.motorMapping[0], NULL, "Off|0|1|2|3|4|5|6|7",
    "Motor 1 =", "m1", &byteType, &_config.base.motorMapping[1], NULL, "Off|0|1|2|3|4|5|6|7",
    "Motor 2 =", "m2", &byteType, &_config.base.motorMapping[2], NULL, "Off|0|1|2|3|4|5|6|7",
    "Motor 3 =", "m3", &byteType, &_config.base.motorMapping[3], NULL, "Off|0|1|2|3|4|5|6|7",
    "Motor 4 =", "m4", &byteType, &_config.base.motorMapping[4], NULL, "Off|0|1|2|3|4|5|6|7",
    "Motor 5 =", "m5", &byteType, &_config.base.motorMapping[5], NULL, "Off|0|1|2|3|4|5|6|7",
    "Motor 6 =", "m6", &byteType, &_config.base.motorMapping[6], NULL, "Off|0|1|2|3|4|5|6|7",
    "Motor 7 =", "m7", &byteType, &_config.base.motorMapping[7], NULL, "Off|0|1|2|3|4|5|6|7",
};

const struct configValues_t flightModeValues[] = {
    "Off - Channel 1"       ,"oC1"    , &byteType         , &_config.base.flightmode[0].channel1, NULL, "0|1|2|3|4|5|6|7",
    "Range a" ,"offC1r"   , &doubleShortType  , &_config.base.flightmode[0].range1, NULL, NULL,
    "Off - Channel 2"       ,"oC2"    , &byteType         , &_config.base.flightmode[0].channel2, NULL, "0|1|2|3|4|5|6|7",
    "Range b" ,"offC2r"   , &doubleShortType  , &_config.base.flightmode[0].range2, NULL, NULL,
    "Acro - Channel 1"      ,"aC1"   , &byteType         , &_config.base.flightmode[1].channel1, NULL, "0|1|2|3|4|5|6|7",
    "Range c","acroC1r"  , &doubleShortType  , &_config.base.flightmode[1].range1, NULL, NULL,
    "Acro - Channel 2"      ,"aC2"   , &byteType         , &_config.base.flightmode[1].channel2, NULL, "0|1|2|3|4|5|6|7",
    "Range d","acroC2r"  , &doubleShortType  , &_config.base.flightmode[1].range2, NULL, NULL,
    "Stab - Channel 1"      ,"sC1"   , &byteType         , &_config.base.flightmode[2].channel1, NULL, "0|1|2|3|4|5|6|7",
    "Range e","stabC1r"  , &doubleShortType  , &_config.base.flightmode[2].range1, NULL, NULL,
    "Stab - Channel 2"      ,"sC2"   , &byteType         , &_config.base.flightmode[2].channel2, NULL, "0|1|2|3|4|5|6|7",
    "Range f","stabC2r"  , &doubleShortType  , &_config.base.flightmode[2].range2, NULL, NULL,
    "AltH - Channel 1"      ,"lC1"    , &byteType         , &_config.base.flightmode[3].channel1, NULL, "0|1|2|3|4|5|6|7",
    "Range g","altC1r"   , &doubleShortType  , &_config.base.flightmode[3].range1, NULL, NULL,
    "AltH - Channel 2"      ,"lC2"    , &byteType         , &_config.base.flightmode[3].channel2, NULL, "0|1|2|3|4|5|6|7",
    "Range h","altC2r"   , &doubleShortType  , &_config.base.flightmode[3].range2, NULL, NULL
};

const struct configValues_t sensorMpuValues[] = {
    "Frequency (hz)","f"                      ,&ushortType    ,&_config.mpu6050.hardwareFreq         ,&mpu6050FreqChanged, NULL,
    "Hardware LPF (hz - gyro/acc)","lp"         ,&byteType      ,&_config.mpu6050.hardwareLPF			,&mpu6050LPFChanged, "None (8khz sample rate)|184/188/|98/94|42/44|20/21|10/10|5/5",
    "Hardware Gyro range (deg/sec)","gr"  ,&byteType      ,&_config.mpu6050.hardwareGyroRange	,&mpu6050GyroRangeChanged, "250|500|1000|2000",
    "Hardware Acc range (G)","ar"          ,&byteType      ,&_config.mpu6050.hardwareAccRange		,&mpu6050AccRangeChanged, "2|4|8|16",
	"Gyro soft avg (samples, XY / Z)","ga",&doubleByteType,&_config.mpu6050.gyroSoftwareAvg		,NULL,NULL,
	"Acc soft avg (samples)","aa"          ,&byteType      ,&_config.mpu6050.accSoftwareAvg		,NULL,NULL,
	"Acc gravity vector","ag"               ,&q32VectorType ,&_config.mpu6050.accCalibration		,&stabGravChanged, NULL,
	"Acc offsets","ao"                    ,&q32VectorType ,&_config.mpu6050.accOffset            ,NULL, NULL,
	"Acc scale","as"                       ,&q32VectorType ,&_config.mpu6050.accScale             ,NULL, NULL
};

const struct configValues_t sensorHmcValues[] = {
    "Frequency (hz)","f"            ,&byteType       ,&_config.hmc5883.hardwareFreq                  ,&magnChangedFreq,  "0.75|1.5|3|7.5|15|30|75",
    "Gain (Gauss/LBs)","g"          ,&byteType       ,&_config.hmc5883.hardwareGain                  ,&magnChangedGain, "1370|1090|820|660|440|390|330|230",
    "Hardware avg (samples)","hw"   ,&byteType       ,&_config.hmc5883.hardwareAvg                   ,&magnChangedAvg, "1 (off)|2|4|8",
    "Software avg (samples)","sw"   ,&byteType       ,&_config.hmc5883.softwareAvg                   ,NULL, NULL
};

const struct configValues_t sensorMplValues[] = {
    "Frequency (hz)", "f"                    ,&ushortType    ,&_config.mpl3115.hardwareFreq, NULL, NULL,
    "Mode", "m"                    ,&byteType    ,&_config.mpl3115.hardwareMode, NULL, "Raw|Altimeter|Barometer",
    "Oversampling (samples)","hw", &byteType     ,&_config.mpl3115.hardwareOversampling, NULL, "1|2|4|8|16|32|64|128",
    "Software avg (samples)","sw"            ,&byteType      ,&_config.mpl3115.softwareAvg, NULL, NULL
};

const struct configValues_t pidValues[] = {
    "P/I/D"                     ,"v"          ,&q32VectorType     ,&_config.pid[0].P, NULL, NULL,
    "I limit"                   ,"L"          ,&q32Type           ,&_config.pid[0].I_limit, NULL, NULL,
    "Setpoint"                  ,"s"          ,&byteType          ,&_config.pid[0].setpointRef, &pidChanged, pidValueReferencesNames,
    "Setpoint Translate"        ,"sT"         ,&q32Type           ,&_config.pid[0].setpointTranslate,NULL,NULL,
    "Setpoint Scale"            ,"sS"         ,&q32Type           ,&_config.pid[0].setpointScale,NULL,NULL,
    "Setpoint Clip"             ,"sC"         ,&q32DoubleType     ,&_config.pid[0].setpointClip,NULL,NULL,
    "Feedback"                  ,"f"          ,&byteType          ,&_config.pid[0].feedbackRef, &pidChanged, pidValueReferencesNames,
    "Feedback Translate"        ,"fT"         ,&q32Type           ,&_config.pid[0].feedbackTranslate,NULL,NULL,
    "Feedback Scale"            ,"FS"         ,&q32Type           ,&_config.pid[0].feedbackScale,NULL,NULL,

    "Output Clip"               ,"o"          ,&q32DoubleType     ,&_config.pid[0].outputClip,NULL,NULL,
    "Motor output"            ,"M"            ,&q32HexType        ,&_config.pid[0].motorMix[0],NULL,NULL,
    "Flight Mode"               ,"fm"         ,&byteBitMaskType   ,&_config.pid[0].modeActive,NULL,NULL
};

 struct configTab_t tabs[] = {
    "MPU6050","mpu"                 ,(struct configValues_t *)sensorMpuValues, sizeof(sensorMpuValues)/sizeof(struct configValues_t),0,
    "HMC5883","hmc"                 ,(struct configValues_t *)sensorHmcValues, sizeof(sensorHmcValues)/sizeof(struct configValues_t),0,
    "MPL3115","mpl"                 ,(struct configValues_t *)sensorMplValues, sizeof(sensorMplValues)/sizeof(struct configValues_t),0,
    "System","sys"                  ,(struct configValues_t *)systemValues, sizeof(systemValues)/sizeof(struct configValues_t),0,
    "Motor mapping","mma"           ,(struct configValues_t *)motorMappingValues, sizeof(motorMappingValues)/sizeof(struct configValues_t),0,
    "Flightmodes","fli"             ,(struct configValues_t *)flightModeValues, sizeof(flightModeValues)/sizeof(struct configValues_t),0,
    "PID[0]","p0"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 0,
    "PID[1]","p1"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 1,
    "PID[2]","p2"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 2,
    "PID[3]","p3"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 3,
    "PID[4]","p4"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 4,
    "PID[5]","p5"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 5,
    "PID[6]","p6"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 6,
    "PID[7]","p7"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 7,
    "PID[8]","p8"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 8,
    "PID[9]","p9"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 9,
    "PID[10]","p10"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 10,
    "PID[11]","p11"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 11,
    "PID[12]","p12"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 12,
    "PID[13]","p13"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 13,
    "PID[14]","p14"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 14,
    "PID[15]","p15"                  ,(struct configValues_t *)pidValues, sizeof(pidValues)/sizeof(struct configValues_t),sizeof(struct PidConfig_t) * 15,
};

struct configTab_t * configGetTabRow(const char * tabName)
{
    int tabIndex = 0;

    for (tabIndex = 0; tabIndex < sizeof(tabs) / sizeof(struct configTab_t); tabIndex++)
    {
        if (strcmp(tabs[tabIndex].name, tabName) == 0) return &tabs[tabIndex];
        if (strcmp(tabs[tabIndex].shortName, tabName) == 0) return &tabs[tabIndex];
    }

    return NULL;
}

struct configValues_t * configGetValueRow(const char * tabName, const char * valueName)
{
    struct configTab_t * tab = configGetTabRow(tabName);
    if (tab == NULL) return NULL;

    int i;
    for (i=0;i<tab->valueLength;i++)
    {
        if (strcmp(tab->values[i].name, valueName) == 0) return &tab->values[i];
        if (strcmp(tab->values[i].shortName, valueName) == 0) return &tab->values[i];
    }

    return NULL;
}

void configSetValue(const char * tabName, const char * valueName, const void * value)
{
    struct configValues_t * val = configGetValueRow(tabName, valueName);
    struct configTab_t * tab = configGetTabRow(tabName);

    if (val == NULL) return;

    if (val->onChange != NULL)
        (*(val->onChange))(val->target + tab->offset, value);
    else
        memcpy(val->target + tab->offset, value, val->type->totalBytes);
}

int configWriteSD(unsigned char * buffer, int pos)
{
    int tabIndex;

    for (tabIndex = 0; tabIndex < sizeof(tabs) / sizeof(struct configTab_t); tabIndex++)
    {
        int i;

        pos = configFillString(buffer, pos, tabs[tabIndex].shortName);

        for (i = 0; i < tabs[tabIndex].valueLength; i++)
        {
            struct configValues_t * cfg = &tabs[tabIndex].values[i];

            pos = configFillString(buffer, pos, cfg->shortName);
            buffer[pos++] = cfg->type->totalBytes;
            buffer[pos++] = cfg->type->numValues;
            buffer[pos++] = cfg->type->intBits;
            buffer[pos++] = cfg->type->floatBits;
            buffer[pos++] = cfg->type->valueType;

            int length = cfg->type->totalBytes;

            memcpy(buffer+pos, cfg->target + tabs[tabIndex].offset, length);

            pos += length;
        }

        buffer[pos++] = 0x00;
    }
}

void configReadSD(unsigned char * buffer, int pos)
{
    struct configTab_t * currentTab;
    struct configValues_t * currentVal;

    while (buffer[pos] != 0x00)
    {
        currentTab = configGetTabRow(buffer+pos);

        // skip over tab-name
        while (buffer[pos] != 0x00) pos++;
        pos++;

        while (buffer[pos] != 0x00)
        {
            currentVal = NULL;
            if (currentTab != NULL)
            {
                currentVal = configGetValueRow(currentTab->shortName, buffer+pos);
            }

            // skip over val-name
            while (buffer[pos] != 0x00) pos++;
            pos++;

            char valType[5];

            valType[0] = buffer[pos++];
            valType[1] = buffer[pos++];
            valType[2] = buffer[pos++];
            valType[3] = buffer[pos++];
            valType[4] = buffer[pos++];

            if (currentVal != NULL)
            {
                if (valType[0] == currentVal->type->totalBytes &&
                    valType[1] == currentVal->type->numValues &&
                    valType[2] == currentVal->type->intBits &&
                    valType[3] == currentVal->type->floatBits &&
                    valType[4] == currentVal->type->valueType)
                {
                    memcpy(currentVal->target + currentTab->offset, buffer+pos, currentVal->type->totalBytes);
                }
            }

            pos += valType[0];
        }

        pos++;
    }
}


int configFillString(unsigned char * buffer, int pos, const char * str)
{
    do
    {
        buffer[pos++] = *str;
    } while (*(str++) != 0x00);
    return pos;
}

void configSendValue(const char * tabName, const char * name)
{
    struct configTab_t * tab = configGetTabRow(tabName);
    struct configValues_t * cfg = configGetValueRow(tabName, name);

    if (cfg == NULL) return;

    if (!rnClaimTx(configTICK_RATE_HZ)) return;
    // TODO: Claim bus first.
    BYTE * packet = rnGetTxBuffer();

    int pos = 0;
    packet[pos++] = CMD_CONFIG_VALUE;
    pos = configFillString(packet, pos, tab->name);
    pos = configFillString(packet, pos, cfg->name);
    packet[pos++] = cfg->type->totalBytes;
    packet[pos++] = cfg->type->numValues;
    packet[pos++] = cfg->type->intBits;
    packet[pos++] = cfg->type->floatBits;
    packet[pos++] = cfg->type->valueType;
    if (cfg->enumValues != NULL)
        pos = configFillString(packet, pos, cfg->enumValues);
    else packet[pos++] = 0;

    int length = cfg->type->totalBytes;

    memcpy(packet+pos, cfg->target + tab->offset, length);

    pos += length;

    rnTransmitAndRelease(packet, pos);

    //rnReleaseTx();
}

void configSendAll()
{
    int tabIndex;

    for (tabIndex = 0; tabIndex < sizeof(tabs) / sizeof(struct configTab_t); tabIndex++)
    {
        int i;

        for (i = 0; i < tabs[tabIndex].valueLength; i++)
        {
            configSendValue(tabs[tabIndex].name, tabs[tabIndex].values[i].name);
        }
    }

    if (!rnClaimTx(configTICK_RATE_HZ)) return;

    BYTE * buffer = rnGetTxBuffer();

    buffer[0] = CMD_CONFIG_VALUE;
    buffer[1] = 0;

    rnTransmitAndConfirmBlock(buffer, 2);

    rnReleaseTx();
}

void configDefault()
{
    _config.imu.freq                        = 1;

	_config.hmc5883.hardwareFreq			= 6; // = 75 hz
	_config.hmc5883.hardwareAvg				= 3; // = 8 samples
	_config.hmc5883.hardwareGain			= 1; // = 1080 gauss/lbs
	_config.hmc5883.softwareAvg             = 1; // no soft

    _config.mpl3115.hardwareOversampling    = 0; // no oversample
    _config.mpl3115.hardwareMode            = 0; // RAW!
	_config.mpl3115.hardwareFreq			= 3; // ?
	_config.mpl3115.softwareAvg             = 1;
    
	_config.mpu6050.hardwareFreq			= 100;
	_config.mpu6050.accSoftwareAvg			= 1;
	_config.mpu6050.accCalibration          = (q_vector_t){0,0,0};
	_config.mpu6050.gyroSoftwareAvg[0]		= 1;
    _config.mpu6050.gyroSoftwareAvg[1]		= 1;
	_config.mpu6050.hardwareLPF				= 0;
	_config.mpu6050.accOffset				= (q_vector_t){0,0,0};
	_config.mpu6050.accScale				= (q_vector_t){Q_ONE,Q_ONE,Q_ONE};
}
